A Sliding Window Filter for SLAM

This note describes a Sliding Window Filter that is an on-line constanttime approximation to the feature-based 6-degree-of-freedom full Batch Least Squares Simultaneous Localization and Mapping (SLAM) problem. We contend that for SLAM to be useful in large environments and over extensive run-times, its computational time complexity must be constant, and its memory requirements should be at most linear. Under this constraint, the “best” algorithm will be the one that comes closest to matching the all-time maximum-likelihood estimate of the full SLAM problem, while also maintaining consistency. We start by formulating SLAM as a Batch Least Squares state estimation problem, and then show how to modify the Batch estimator into an approximate Sliding Window Batch/Recursive framework that achieves constant time complexity and linear space complexity. We argue that viewing SLAM from the Sliding Window Least Squares perspective is very useful for understanding the structure of the problem. This perspective is general, capable of subsuming a number of common estimation techniques such as Bundle Adjustment and Extended Kalman Filter SLAM. By tuning the sliding window, the algorithm can scale from exhaustive Batch solutions to fast incremental solutions; if the window encompasses all time, the solution is algebraically equivalent to full SLAM; if only one time step is maintained, the solution is algebraically equivalent to the Extended Kalman Filter SLAM solution. The Sliding Window Filter enables other interesting properties, like continuous sub-mapping, lazy data association, undelayed or delayed landmark initialization, and incremental robust estimation. We test the algorithm in simulations using stereo vision exterioceptive sensors and inertial measurement proprioceptive sensors. Initial experiments show that the SWF approaches the performance of the optimal batch estimator, even for small windows on the order of 5-10 frames.